#include <math.h>
#include <stdint.h>
#include "wk_tmr.h"
#include "FOC_transform.h"
#include "CurrentSenser.h"
#include "MotorDriver.h"

#if 0

#define M_PI 3.1415926f

// PWM最大值
#define PWM_MAX 3000
// 相位差 (120度)
#define PHASE_SHIFT (2.0 * M_PI / 3.0)
// 正弦波频率 (Hz)
#define SINE_FREQ 7
// 采样频率 (Hz)
#define SAMPLE_FREQ 100
// 幅值系数 (0.0-1.0)
#define AMPLITUDE 0.5

// 全局变量
static float angle = 0.0f;
static float angle_increment = 2.0f * M_PI * SINE_FREQ / SAMPLE_FREQ;

// 初始化三相正弦发生器
void three_phase_sine_init(void)
{
    angle = 0.0f;
}

// 更新三相PWM输出
void three_phase_sine_update(void)
{
    // 计算三相正弦值 (-1.0 到 1.0)
    float sine_u = sinf(angle);
    float sine_v = sinf(angle + PHASE_SHIFT);
    float sine_w = sinf(angle + 2 * PHASE_SHIFT);

    // 将正弦值转换为PWM占空比 (0到PWM_MAX)
    uint16_t pwm_u = (uint16_t)((sine_u * AMPLITUDE + 1.0f) * PWM_MAX / 2);
    uint16_t pwm_v = (uint16_t)((sine_v * AMPLITUDE + 1.0f) * PWM_MAX / 2);
    uint16_t pwm_w = (uint16_t)((sine_w * AMPLITUDE + 1.0f) * PWM_MAX / 2);

    // 设置PWM占空比
    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, pwm_u);
    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_v);
    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_3, pwm_w);

    // 更新角度，确保在0-2π范围内
    angle += angle_increment;
    if (angle >= 100.0f * M_PI)
    {
        angle = 0;
    }
}

void foc_test(void)
{
    static uint8_t led = 0;
    float angle = 0;
    float alpha, beta;
    float d = 0, q = 2;
    float v[3];
    uint16_t Ib_raw, Ic_raw;
    static float Ib, Ic, Ib_l = 0, Ic_l = 0;

    uint16_t Ib_raw_2, Ic_raw_2;
    static float Ib_2, Ic_2, Ib_l_2 = 0, Ic_l_2 = 0;

    FOC_elec_Angle(&angle);                                              // 获取电角度
    FOC_park_transform(TRANFORM_INVERSE, &angle, &alpha, &beta, &d, &q); // Park 变换
    SVPWM(alpha, beta, v);                                               // 计算调制占空比

    // FOC_clark_transform(TRANFORM_INVERSE, &v[0], &v[1], &v[2], &alpha, &beta);
    // v[0] = 2 * cosf(angle);                // 计算电压
    // v[1] = 2 * cosf(angle + 2 / 3 * M_PI); // 计算电压
    // v[2] = 2 * cosf(angle - 2 / 3 * M_PI); // 计算电压
    MotorDriver_set_volatge_three(v[0], v[1], v[2]);

    // CurrentSenser_get_current(0, &Ic_raw, &Ic); // 获取 C 相电流
    // CurrentSenser_get_current(1, &Ib_raw, &Ib); // 获取 B 相电流

    CurrentSenser_get_current(2, &Ic_raw_2, &Ic_2); // 获取 C 相电流
    CurrentSenser_get_current(3, &Ib_raw_2, &Ib_2); // 获取 B 相电流

    // Ib = 0.2 * Ib + 0.8 * Ib_l;
    // Ic = 0.2 * Ic + 0.8 * Ic_l; // 简单低通滤波
    // Ib_l = Ib;
    // Ic_l = Ic;
    printf("%.2f,%.2f,", Ib_2 * 10, Ic_2 * 10);

    Ib_2 = 0.5 * (Ib_2 + Ib_l_2);
    Ic_2 = 0.5 * (Ic_2 + Ic_l_2); // 简单低通滤波
    Ib_l_2 = Ib_2;
    Ic_l_2 = Ic_2;

    if (led)
    {
        gpio_bits_set(GPIOC, GPIO_PINS_13);
        led = 0;
    }
    else
    {
        gpio_bits_reset(GPIOC, GPIO_PINS_13);
        led = 1;
    }

    // printf("I:%.2f,%.2f,%.2f,%u,%u,0\r\n", -(Ib + Ic) * 10, Ib * 10, Ic * 10, Ib_raw, Ic_raw);

    printf("%.2f,%.2f,0\r\n", Ib_2 * 10, Ic_2 * 10);
}
#endif 
